import pygame
import math
import time
WIDTH = 800
HEIGHT = 600

class Robot:
    def __init__(self,x,y,orientation):
        self.x = x
        self.y = y
        self.orientation = orientation
        self.dt = 0.1
        self.v = 0
        self.w = 0
    def motion(self,v,w):
        self.orientation += w*self.dt
        self.orientation = self.orientation %360
        print(self.orientation)
        self.x += v*math.cos(math.radians(self.orientation))*self.dt
        self.y += v*math.sin(math.radians(self.orientation))*self.dt
        self.v = v
        self.w = w


    def move_forward(self,distance):
        self.x += distance * math.cos(math.radians(self.orientation))
        self.y += distance * math.sin(math.radians(self.orientation))
    
    def rotate(self,angle):
        self.orientation = (self.orientation + angle)%360
    
    def draw(self,screen):
        triangle_points = [(self.x+math.cos(math.radians(self.orientation+45))*20   ,self.y + math.sin(math.radians(self.orientation+45))*20),
                           (self.x+math.cos(math.radians(self.orientation+135))*20  ,self.y + math.sin(math.radians(self.orientation+135))*20),
                           (self.x+math.cos(math.radians(self.orientation+225))*20  ,self.y + math.sin(math.radians(self.orientation+225))*20),
                           (self.x+math.cos(math.radians(self.orientation+315))*20  ,self.y + math.sin(math.radians(self.orientation+315))*20)]
        pygame.draw.polygon(screen,(255,0,0),triangle_points)
    
# 初始化Pygame
pygame.init()

screen = pygame.display.set_mode((WIDTH,HEIGHT))
canvas = pygame.Surface((WIDTH,HEIGHT))

robot = Robot(WIDTH//2,HEIGHT//2,0)

running = True
while running:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False
    canvas.fill((255,255,255))

    # robot.move_forward(4)
    # robot.rotate(1)
    robot.motion(0,60)
    robot.draw(canvas)
    screen.blit(canvas,(0,0))

    pygame.display.flip()
    time.sleep(0.01)

pygame.quit